/*
 * Copyright 2016 The slam Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "slam_ros/node.h"

#include <chrono>
#include <fstream>
#include <memory>
#include <sstream>
#include <string>
#include <thread>
#include <vector>

#include "Eigen/Core"
#include "absl/memory/memory.h"
#include "absl/strings/str_cat.h"
#include "agv_msgs/msg/confidence_level.hpp"
#include "agv_msgs/msg/odometry_lite.hpp"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "slam/common/configuration_file_resolver.h"
#include "slam/common/lua_parameter_dictionary.h"
#include "slam/common/port.h"
#include "slam/common/time.h"
#include "slam/io/proto_stream.h"
#include "slam/io/proto_stream_deserializer.h"
#include "slam/io/submap_painter.h"
#include "slam/mapping/2d/probability_grid.h"
#include "slam/mapping/2d/submap_2d.h"
#include "slam/mapping/3d/submap_3d.h"
#include "slam/mapping/pose_graph_interface.h"
#include "slam/mapping/proto/submap_visualization.pb.h"
#include "slam/metrics/register.h"
#include "slam/sensor/point_cloud.h"
#include "slam/transform/rigid_transform.h"
#include "slam/transform/transform.h"
#include "slam_ros/metrics/family_factory.h"
#include "slam_ros/msg_conversion.h"
#include "slam_ros/ros_map.h"
#include "slam_ros/sensor_bridge.h"
#include "slam_ros/tf_bridge.h"
#include "slam_ros/time_conversion.h"
#include "slam_ros/utilities.h"
#include "slam_ros_msgs/msg/landmark_list_pose.hpp"
#include "slam_ros_msgs/msg/robot_status.hpp"
#include "slam_ros_msgs/msg/status_code.hpp"
#include "slam_ros_msgs/msg/status_response.hpp"
#include "tf2_eigen/tf2_eigen.hpp"
#include "visualization_msgs/msg/marker_array.hpp"

namespace slam_ros {

using slam::transform::Rigid3d;
using TrajectoryState = ::slam::mapping::PoseGraphInterface::TrajectoryState;

namespace {
// Subscribes to the 'topic' for 'trajectory_id' using the 'node' and
// calls 'handler' on the 'node' to handle messages. Returns the subscriber.
template <typename MessageType>
::rclcpp::SubscriptionBase::SharedPtr SubscribeWithHandler(
    void (Node::*handler)(int, const std::string&,
                          const typename MessageType::ConstSharedPtr),
    const int trajectory_id, const std::string& topic, Node* const node) {
  return node->create_subscription<MessageType>(
      topic, rclcpp::SensorDataQoS(),
      [node, handler, trajectory_id,
       topic](const typename MessageType::ConstSharedPtr msg) {
        (node->*handler)(trajectory_id, topic, msg);
      });
}

std::string TrajectoryStateToString(const TrajectoryState trajectory_state) {
  switch (trajectory_state) {
    case TrajectoryState::ACTIVE:
      return "ACTIVE";
    case TrajectoryState::FINISHED:
      return "FINISHED";
    case TrajectoryState::FROZEN:
      return "FROZEN";
    case TrajectoryState::DELETED:
      return "DELETED";
  }
  return "";
}

}  // namespace

Node::Node(const NodeOptions& node_options,
           std::unique_ptr<slam::mapping::MapBuilderInterface> map_builder,
           tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
    : rclcpp::Node("slam_node"),
      node_options_(node_options),
      map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer),
      tf_buffer_(tf_buffer) {
  tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

  map_builder_bridge_.SetPosePublishCallback(
      [this](const ::slam::common::Time time,
             const ::slam::transform::Rigid3d T_map_odom,
             const slam::transform::Rigid3d odom_pose,
             const ::slam::mapping::ReflectorMatchResult
                 reflector_match_result) {
        PublishPose(time, T_map_odom, odom_pose, reflector_match_result);
      });

  absl::MutexLock lock(&mutex_);
  if (collect_metrics) {
    metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
    slam::metrics::RegisterAllMetrics(metrics_registry_.get());
  }

  agv_mode_sub_ =  this->create_subscription<agv_msgs::msg::AGVMode>(
            "/agvmode", 
            1, 
            std::bind(&Node::HandleAGVMode, this, std::placeholders::_1));

  // Initialize publishers
  submap_list_publisher_ =
      this->create_publisher<slam_ros_msgs::msg::SubmapList>(
          kSubmapListTopic, rclcpp::SystemDefaultsQoS());
  trajectory_node_list_publisher_ =
      this->create_publisher<visualization_msgs::msg::MarkerArray>(
          kTrajectoryNodeListTopic, rclcpp::SystemDefaultsQoS());
  landmark_poses_list_publisher_ =
      this->create_publisher<visualization_msgs::msg::MarkerArray>(
          kLandmarkPosesListTopic, rclcpp::SystemDefaultsQoS());
  agv_landmark_poses_list_publisher_ =
      this->create_publisher<slam_ros_msgs::msg::LandmarkListPose>(
          kAgvLandmarkTopicName, rclcpp::SystemDefaultsQoS());
  constraint_list_publisher_ =
      this->create_publisher<visualization_msgs::msg::MarkerArray>(
          kConstraintListTopic, rclcpp::SystemDefaultsQoS());
  robot_pose_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
      "localization_pose", rclcpp::SystemDefaultsQoS());
  robot_pose_with_confidence_pub_ =
      this->create_publisher<geometry_msgs::msg::PoseStamped>(
          kPoseWithConfidenceTopicName, rclcpp::SystemDefaultsQoS());
  fusion_laserpose_pub_ = this->create_publisher<agv_msgs::msg::PoseWithConfidence>(
      "fusion_laser_pose", rclcpp::SystemDefaultsQoS());
      
  pose_with_confidence_pub_ =
      this->create_publisher<agv_msgs::msg::PoseWithConfidence>(
          kagvPoseWithConfidenceTopicName, rclcpp::SystemDefaultsQoS());
          
  cur_confidence_pub_ =
      this->create_publisher<agv_msgs::msg::ConfidenceLevel>(
          kagvConfidenceTopicName, rclcpp::SystemDefaultsQoS());
          
  agv_robot_status_pub_ =
      this->create_publisher<agv_msgs::msg::PoseWithConfidence>(
          kAgvRobotStatus, rclcpp::SystemDefaultsQoS());
          
  slam_event_pub_ = this->create_publisher<agv_msgs::msg::AGVEvent>(
      "/agvevent", rclcpp::QoS(1));
  scan_matched_point_cloud_publisher_ =
      this->create_publisher<sensor_msgs::msg::PointCloud2>(
          kScanMatchedPointCloudTopic, rclcpp::SystemDefaultsQoS());
  robot_matched_point_cloud_publisher_ =
      this->create_publisher<sensor_msgs::msg::PointCloud>(
          kRobotMatchedPointCloudTopic, rclcpp::SystemDefaultsQoS());
  robot_status_ = this->create_publisher<slam_ros_msgs::msg::RobotStatus>(
      kRobotStatus, rclcpp::SystemDefaultsQoS());
  // ... reflector_landmark_pub_ needs to be initialized
  reflector_landmark_pub_ =
      this->create_publisher<slam_ros_msgs::msg::ReflectorLandmark>(
          "reflector_landmark", rclcpp::SystemDefaultsQoS());

  // Initialize services
  service_servers_.push_back(
      this->create_service<slam_ros_msgs::srv::SubmapQuery>(
          kSubmapQueryServiceName,
          std::bind(&Node::HandleSubmapQuery, this, std::placeholders::_1,
                    std::placeholders::_2)));
  service_servers_.push_back(
      this->create_service<slam_ros_msgs::srv::TrajectoryQuery>(
          kTrajectoryQueryServiceName,
          std::bind(&Node::HandleTrajectoryQuery, this, std::placeholders::_1,
                    std::placeholders::_2)));
  service_servers_.push_back(
      this->create_service<slam_ros_msgs::srv::StartTrajectory>(
          kStartTrajectoryServiceName,
          std::bind(&Node::HandleStartTrajectory, this, std::placeholders::_1,
                    std::placeholders::_2)));
  service_servers_.push_back(
      this->create_service<slam_ros_msgs::srv::FinishTrajectory>(
          kFinishTrajectoryServiceName,
          std::bind(&Node::HandleFinishTrajectory, this, std::placeholders::_1,
                    std::placeholders::_2)));
  service_servers_.push_back(
      this->create_service<slam_ros_msgs::srv::WriteState>(
          kWriteStateServiceName,
          std::bind(&Node::HandleWriteState, this, std::placeholders::_1,
                    std::placeholders::_2)));
  service_servers_.push_back(
      this->create_service<slam_ros_msgs::srv::GetTrajectoryStates>(
          kGetTrajectoryStatesServiceName,
          std::bind(&Node::HandleGetTrajectoryStates, this,
                    std::placeholders::_1, std::placeholders::_2)));
  service_servers_.push_back(
      this->create_service<slam_ros_msgs::srv::ReadMetrics>(
          kReadMetricsServiceName,
          std::bind(&Node::HandleReadMetrics, this, std::placeholders::_1,
                    std::placeholders::_2)));
  service_servers_.push_back(
      this->create_service<slam_ros_msgs::srv::OnlyUseOdomPredictPose>(
          kSetOnlyUseOdomPredictPoseFlag,
          std::bind(&Node::HandleOnlyUseOdomPredictPoseFlag, this,
                    std::placeholders::_1, std::placeholders::_2)));
  service_servers_.push_back(
      this->create_service<slam_ros_msgs::srv::LogSlamData>(
          kSlamLogData, std::bind(&Node::HandleLogSlamData, this,
                                  std::placeholders::_1,
                                  std::placeholders::_2)));
  service_servers_.push_back(
      this->create_service<slam_ros_msgs::srv::ControlSam>(
          kControlSlam, std::bind(&Node::HandleControlSLAM, this,
                                  std::placeholders::_1,
                                  std::placeholders::_2)));
  service_servers_.push_back(
      this->create_service<slam_ros_msgs::srv::PgmToLocalizationMap>(
          kPgmToLocalizationMap,
          std::bind(&Node::HandlePgmToLocalizationMap, this,
                    std::placeholders::_1, std::placeholders::_2)));
  service_servers_.push_back(
      this->create_service<slam_ros_msgs::srv::GetLandmarkLists>(
          kGetLandmarkListsTask,
          std::bind(&Node::HandleLandMarkLists, this, std::placeholders::_1,
                    std::placeholders::_2)));

  // Initialize timers
  submap_list_timer_ = this->create_wall_timer(
      std::chrono::duration<double>(node_options_.submap_publish_period_sec),
      std::bind(&Node::PublishSubmapList, this));
  trajectory_node_list_timer_ = this->create_wall_timer(
      std::chrono::duration<double>(
          node_options_.trajectory_publish_period_sec),
      std::bind(&Node::PublishTrajectoryNodeList, this));
  landmark_poses_list_timer_ = this->create_wall_timer(
      std::chrono::duration<double>(
          node_options_.trajectory_publish_period_sec),
      std::bind(&Node::PublishLandmarkPosesList, this));
  constraint_list_timer_ = this->create_wall_timer(
      std::chrono::duration<double>(kConstraintPublishPeriodSec),
      std::bind(&Node::PublishConstraintList, this));
  log_robot_pose_timer_ = this->create_wall_timer(
      std::chrono::duration<double>(0.5), std::bind(&Node::LogRobotPose, this));
  publish_slam_error_timer_ = this->create_wall_timer(
      std::chrono::duration<double>(1),
      std::bind(&Node::PublishSlamError, this));

  if (node_options_.pose_publish_period_sec > 0) {
    publish_local_trajectory_data_timer_ = this->create_wall_timer(
        std::chrono::duration<double>(node_options_.pose_publish_period_sec),
        std::bind(&Node::PublishLocalTrajectoryData, this));
  }

  // Initialize sensor data cache
  sensor_data_cache_ = absl::make_unique<SensorDataCache>(
      node_options_.sensor_data_log_path,
      node_options_.cache_data_time_duration_sec);

  log_sensor_data_ = false;
  stop_sensor_data_cache_thread_ = false;
  sensor_data_cache_thread_ =
      absl::make_unique<std::thread>(&Node::ProcessLogSensorData, this);

  // Create map directory
  CHECK(kMapPath.back() == '/');
  if (!dirExists(kMapPath)) {
    RCLCPP_ERROR(this->get_logger(), "Map path directory can not be found: %s",
                 kMapPath.c_str());
    RCLCPP_INFO(this->get_logger(), "Create map_path directory: %d",
                mkPath(kMapPath, 0777));
    std::this_thread::sleep_for(std::chrono::milliseconds(500));
  }
}

Node::~Node() {
  FinishAllTrajectories();
  {
    absl::MutexLock lock(&mutex_);
    stop_sensor_data_cache_thread_ = true;
    log_sensor_data_ = false;
    sensor_data_cache_.reset(nullptr);
  }
  if (sensor_data_cache_thread_) {
    sensor_data_cache_thread_->join();
    sensor_data_cache_thread_ = nullptr;
  }
}

void Node::HandleAGVMode(const agv_msgs::msg::AGVMode::ConstPtr& msg) {
  current_agv_mode_ = msg->mode;
}

void Node::HandleSubmapQuery(
    const std::shared_ptr<slam_ros_msgs::srv::SubmapQuery::Request> request,
    std::shared_ptr<slam_ros_msgs::srv::SubmapQuery::Response> response) {
  absl::MutexLock lock(&mutex_);
  map_builder_bridge_.HandleSubmapQuery(request, response);
}

void Node::HandleTrajectoryQuery(
    const std::shared_ptr<slam_ros_msgs::srv::TrajectoryQuery::Request> request,
    std::shared_ptr<slam_ros_msgs::srv::TrajectoryQuery::Response> response) {
  absl::MutexLock lock(&mutex_);
  response->status = TrajectoryStateToStatus(
      request->trajectory_id,
      {TrajectoryState::ACTIVE, TrajectoryState::FINISHED,
       TrajectoryState::FROZEN} /* valid states */);
  if (response->status.code != slam_ros_msgs::msg::StatusCode::OK) {
    RCLCPP_ERROR(this->get_logger(),
                 "Can't query trajectory from pose graph: %s",
                 response->status.message.c_str());
    return;
  }
  map_builder_bridge_.HandleTrajectoryQuery(request, response);
}

void Node::PublishSubmapList() {
  absl::MutexLock lock(&mutex_);
  submap_list_publisher_->publish(map_builder_bridge_.GetSubmapList(this->now()));
}

void Node::AddSensorSamplers(const int trajectory_id,
                             const TrajectoryOptions& options) {
  CHECK(sensor_samplers_.count(trajectory_id) == 0);
  sensor_samplers_.emplace(
      std::piecewise_construct, std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          options.rangefinder_sampling_ratio, options.odometry_sampling_ratio,
          options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio,
          options.landmarks_sampling_ratio,
          options.qr_code_sampling_ratio));
}

void Node::PublishLocalTrajectoryData() {
  absl::MutexLock lock(&mutex_);
  if (slam_cmd_ == 0) return;
  for (const auto& entry : map_builder_bridge_.GetLocalTrajectoryData()) {
    const auto& trajectory_data = entry.second;
    if (trajectory_data.local_slam_data->time != last_pose_time_) {
      // We only publish a point cloud if it has changed. It is not needed at
      // high frequency, and republishing it would be computationally wasteful.
      if (robot_matched_point_cloud_publisher_->get_subscription_count() > 0) {
        // TODO(gaschler): Consider using other message without time
        // information.
        slam::sensor::TimedPointCloud point_cloud;
        point_cloud.reserve(trajectory_data.local_slam_data->range_data_in_local
                                .returns.size());
        for (const slam::sensor::RangefinderPoint point :
             trajectory_data.local_slam_data->range_data_in_local.returns) {
          point_cloud.push_back(
              slam::sensor::ToTimedRangefinderPoint(point, 0.f /* time */));
        }
        robot_matched_point_cloud_publisher_->publish(ToPointCloudMessage(
            slam::common::ToUniversal(trajectory_data.local_slam_data->time),
            node_options_.map_frame,
            slam::sensor::TransformTimedPointCloud(
                point_cloud, (map_init_pose_from_yaml *
                              trajectory_data.local_to_map)
                                 .cast<float>())));
      }
    }
    last_pose_time_ = trajectory_data.local_slam_data->time;

    geometry_msgs::msg::TransformStamped stamped_transform;
    // If we do not publish a new point cloud, we still allow time of the
    // published poses to advance. If we already know a newer pose, we use its
    // time instead. Since tf knows how to interpolate, providing newer
    // information is better.
    stamped_transform.header.stamp =
        ToRos(trajectory_data.local_slam_data->time);
    const Rigid3d tracking_to_local_3d =
        trajectory_data.local_slam_data->local_pose;
    const Rigid3d tracking_to_local = [&] {
      if (trajectory_data.trajectory_options.publish_frame_projected_to_2d) {
        return slam::transform::Embed3D(
            slam::transform::Project2D(tracking_to_local_3d));
      }
      return tracking_to_local_3d;
    }();

    const Rigid3d tracking_to_map =
        trajectory_data.local_to_map * tracking_to_local;

    const slam::mapping::ReflectorMatchResult& reflector_match_result =
        trajectory_data.local_slam_data->reflector_match_result;

    if (trajectory_data.published_to_tracking != nullptr) {
      const Rigid3d current_robot_pose =
          map_init_pose_from_yaml * tracking_to_map *
          (*trajectory_data.published_to_tracking);
      geometry_msgs::msg::PointStamped robot_stamped_pose =
          ToPointStampedPose(current_robot_pose, stamped_transform.header.stamp);
      is_localization_ok_ = map_builder_bridge_.IsLocalizationSuccess();
      if (!is_localization_ok_) {
        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
                             "Slam status: %d", is_localization_ok_);
      }

      const double current_stamp = this->now().seconds();
      const double delta_t_laser = current_stamp - laser_data_time_last_;
      if (delta_t_laser > 1.0 && trajectory_options_.num_laser_scans != 0 &&
          is_scan_initialized_) {
        is_laser_data_ok_ = false;
      }
      is_laser_ok_ = is_laser_data_ok_ && is_slam_laser_data_ok_;
      if (!is_laser_ok_) {
        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
                             "Laser status error.");
      }

      if (trajectory_options_.use_odometry && is_odom_initialized_) {
        const double delta_t_odom = current_stamp - odometry_data_time_last_;
        if (delta_t_odom > 1.0) {
          is_odom_ok_ = false;
        }
      }
      if (!is_odom_ok_) {
        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
                             "Odom status error.");
      }

      if (trajectory_options_.trajectory_builder_options
              .trajectory_builder_2d_options()
              .use_imu_data() &&
          is_imu_initialized_) {
        const double delta_t_imu = current_stamp - imu_data_time_last_;
        if (delta_t_imu > 1.0) {
          is_imu_ok_ = false;
        }
      }
      if (!is_imu_ok_) {
        RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
                             "Imu status error.");
      }

      this->get_parameter("/AGVParam/agv_navigation_type", is_laser_nav);

      if (!trajectory_data.trajectory_options.use_odometry) {
        slam_ros_msgs::msg::RobotStatus robot_status;
        robot_status.slam_type.data = static_cast<int>(slam_type_);
        robot_status.localization_map = current_localization_map_;
        robot_status.laser_to_base = laser_to_base_;
        if (/*!is_localization_ok_ || */
          !is_laser_ok_ /*|| !is_odom_ok_ ||!is_imu_ok_ */) {
          robot_status.localization_state.data = 0;
          robot_status.localization_pose = robot_stamped_pose;
          robot_status_->publish(robot_status);
          return;
        }

        robot_pose_now_ = current_robot_pose;
        sensor_data_cache_->AddRobotPose(std::make_shared<geometry_msgs::msg::PointStamped>(robot_stamped_pose));
		    RCLCPP_INFO(this->get_logger(), "=======PublishLocalTrajectoryData========");

        robot_pose_pub_->publish(robot_stamped_pose);
        robot_pose_with_confidence_pub_->publish(
            ToPoseStamped(robot_pose_now_, robot_stamped_pose.header.stamp));

        if (is_laser_nav == 0) {
          pose_with_confidence_pub_->publish(
              ToTrackedPoseWithConfidence(robot_pose_now_, this->now()));
        }

        robot_status.localization_state.data = 1;

        if (node_options_.min_reflector_fusion_num >= 2 &&
            reflector_match_result.matched_num >=
                node_options_.min_reflector_fusion_num) {
          robot_status.localization_pose = ToPointStampedPose(
              reflector_match_result.robot_pose,
              robot_stamped_pose.header.stamp);
        } else {
          robot_status.localization_pose = robot_stamped_pose;
        }

        robot_status_->publish(robot_status);

        if (reflector_match_result.matched_num >= 1) {
          slam_ros_msgs::msg::ReflectorLandmark reflector_result;
          reflector_result.reflector_num = reflector_match_result.matched_num;
          reflector_result.robot_pose = ToPointStampedPose(
              reflector_match_result.robot_pose,
              this->now());
        slam_ros_msgs::msg::LandmarkListPose landmark_poses;
        for (const auto& item : reflector_match_result.matched_reflector) {
            geometry_msgs::msg::Pose landmark_pose;
            const Eigen::Vector3d& translation = Eigen::Vector3d(item.reflector_in_map.x(),
                                                  item.reflector_in_map.y(), 0);
            const Eigen::Quaterniond& rotation = {1.0, 0.0, 0.0, 0.0};
            ::slam::transform::Rigid3d pose_init =
                ::slam::transform::Rigid3d(translation, rotation);
            const Rigid3d cur_lanmark_pose =
                map_init_pose_from_yaml * pose_init;
            landmark_pose.position.x = cur_lanmark_pose.translation().x();
            landmark_pose.position.y = cur_lanmark_pose.translation().y();
            landmark_poses.g_landmark_pose.push_back(landmark_pose);
            reflector_result.g_landmark_pose.push_back(landmark_pose);
          }
          reflector_landmark_pub_->publish(reflector_result);
          agv_landmark_poses_list_publisher_->publish(landmark_poses);
        }     

        if (trajectory_data.trajectory_options.provide_odom_frame) {
          std::vector<geometry_msgs::msg::TransformStamped> stamped_transforms;

          stamped_transform.header.frame_id = node_options_.map_frame;
          stamped_transform.child_frame_id =
              trajectory_data.trajectory_options.odom_frame;
          stamped_transform.transform =
              ToGeometryMsgTransform(trajectory_data.local_to_map);
          stamped_transforms.push_back(stamped_transform);

          stamped_transform.header.frame_id =
              trajectory_data.trajectory_options.odom_frame;
          stamped_transform.child_frame_id =
              trajectory_data.trajectory_options.published_frame;
          stamped_transform.transform = ToGeometryMsgTransform(
              tracking_to_local * (*trajectory_data.published_to_tracking));
          stamped_transforms.push_back(stamped_transform);

          tf_broadcaster_->sendTransform(stamped_transforms);
        } else {
          stamped_transform.header.frame_id = node_options_.map_frame;
          stamped_transform.child_frame_id =
              trajectory_data.trajectory_options.published_frame;
          stamped_transform.transform = ToGeometryMsgTransform(
              tracking_to_map * (*trajectory_data.published_to_tracking));
          tf_broadcaster_->sendTransform(stamped_transform);
        }
      }
    }
  }
}

void Node::PublishTrajectoryNodeList() {
  if (trajectory_node_list_publisher_->get_subscription_count() > 0) {
    absl::MutexLock lock(&mutex_);
    trajectory_node_list_publisher_->publish(
        map_builder_bridge_.GetTrajectoryNodeList(this->now()));
  }
}

void Node::PublishLandmarkPosesList() {
  absl::MutexLock lock(&mutex_);
  visualization_msgs::msg::MarkerArray LandMarkLists =
      map_builder_bridge_.GetLandmarkPosesList(this->now());

  if (landmark_poses_list_publisher_->get_subscription_count() > 0) {
    landmark_poses_list_publisher_->publish(LandMarkLists);
  }

  // if (agv_landmark_poses_list_publisher_.getNumSubscribers() > 0 &&
  //     !LandMarkLists.markers.empty()) {
  //   for (const auto& landmark : LandMarkLists.markers) {
  //     if (landmark.type != visualization_msgs::Marker::LINE_LIST) continue;

  //     geometry_msgs::Pose landmark_pose;
  //     slam_ros_msgs::LandmarkListPose landmark_poses;

  //     landmark_poses.header = landmark.header;
  //     for (int id = 0; id < landmark.points.size(); ++id) {
  //       if (true/*(id & 1) == 1*/) {
  //         const Eigen::Vector3d& translation = {landmark.points[id].x,
  //                                               landmark.points[id].y, 0};
  //         const Eigen::Quaterniond& rotation = {1.0, 0.0, 0.0, 0.0};
  //         ::slam::transform::Rigid3d pose_init =
  //             ::slam::transform::Rigid3d(translation, rotation);
  //         const Rigid3d cur_lanmark_pose = map_init_pose_from_yaml *
  //         pose_init; landmark_pose.position.x =
  //         cur_lanmark_pose.translation().x(); landmark_pose.position.y =
  //         cur_lanmark_pose.translation().y();
  //         landmark_poses.g_landmark_pose.push_back(landmark_pose);
  //       }
  //     }

  //     agv_landmark_poses_list_publisher_.publish(landmark_poses);
  //   }
  // }
}

void Node::PublishConstraintList() {
  if (constraint_list_publisher_->get_subscription_count() > 0) {
    absl::MutexLock lock(&mutex_);
    constraint_list_publisher_->publish(
        map_builder_bridge_.GetConstraintList(this->now()));
  }
}

std::set<slam::mapping::TrajectoryBuilderInterface::SensorId>
Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const {
  using SensorId = slam::mapping::TrajectoryBuilderInterface::SensorId;
  using SensorType = SensorId::SensorType;
  std::set<SensorId> expected_topics;
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
    expected_topics.insert(SensorId{SensorType::RANGE, topic});
  }
  for (const std::string& topic : ComputeRepeatedTopicNames(
           kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
    expected_topics.insert(SensorId{SensorType::RANGE, topic});
  }
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
    expected_topics.insert(SensorId{SensorType::RANGE, topic});
  }
  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
  // required.
  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data())) {
    expected_topics.insert(SensorId{SensorType::IMU, kImuTopic});
  }
  // Odometry is optional.
  if (options.use_odometry) {
    expected_topics.insert(SensorId{SensorType::ODOMETRY, kOdometryTopic});
  }
  // NavSatFix is optional.
  if (options.use_nav_sat) {
    expected_topics.insert(
        SensorId{SensorType::FIXED_FRAME_POSE, kNavSatFixTopic});
  }
  // Landmark is optional.
  if (options.use_landmarks) {
    expected_topics.insert(SensorId{SensorType::LANDMARK, kLandmarkTopic});
  }
  if (options.use_qr_code) {
    expected_topics.insert(SensorId{SensorType::QRCODE, kQRCodeTopic});
  }
  return expected_topics;
}

int Node::AddTrajectory(const TrajectoryOptions& options) {
  const std::set<slam::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  AddSensorSamplers(trajectory_id, options);
  LaunchSubscribers(options, trajectory_id);
  wall_timers_ = this->create_wall_timer(
      std::chrono::duration<double>(kTopicMismatchCheckDelaySec),
      std::bind(&Node::MaybeWarnAboutTopicMismatch, this));
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }

  trajectory_options_ = options;

  if (!trajectory_options_.use_odometry) {
    is_odom_ok_ = true;
  }
  if (!trajectory_options_.trajectory_builder_options
           .trajectory_builder_2d_options()
           .use_imu_data()) {
    is_imu_ok_ = true;
  }
  if (trajectory_options_.num_laser_scans == 0) {
    is_laser_data_ok_ = true;
    is_laser_ok_ = true;
    is_slam_laser_data_ok_ = true;
  }
  current_trajectory_id_ = trajectory_id;
  robot_pose_seq_ = 0;
  return trajectory_id;
}

void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const int trajectory_id) {
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::msg::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, this),
         topic});
  }
  for (const std::string& topic : ComputeRepeatedTopicNames(
           kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::msg::MultiEchoLaserScan>(
             &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
             this),
         topic});
  }
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::msg::PointCloud2>(
             &Node::HandlePointCloud2Message, trajectory_id, topic, this),
         topic});
  }

  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
  // required.  
  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data())) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::msg::Imu>(
             &Node::HandleImuMessage, trajectory_id, kImuTopic, this),
         kImuTopic});
  }

  if (options.use_odometry) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<agv_msgs::msg::OdometryLite>(
             &Node::HandleOdometryMessage, trajectory_id, kOdometryTopic, this),
         kOdometryTopic});
  }
  if (options.use_nav_sat) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::msg::NavSatFix>(
             &Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
             this),
         kNavSatFixTopic});
  }

  if (options.use_landmarks) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<slam_ros_msgs::msg::LandmarkList>(
             &Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic, this),
         kLandmarkTopic});
  }
  if (options.use_qr_code) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<slam_ros_msgs::msg::QRCode>(
             &Node::HandleQRCodeMessage, trajectory_id, kQRCodeTopic, this),
         kQRCodeTopic});
  }
}

bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
  if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
    return options.trajectory_builder_options
        .has_trajectory_builder_2d_options();
  }
  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
    return options.trajectory_builder_options
        .has_trajectory_builder_3d_options();
  }
  return false;
}

bool Node::ValidateTopicNames(const TrajectoryOptions& options) {
  for (const auto& sensor_id : ComputeExpectedSensorIds(options)) {
    const std::string& topic = sensor_id.id;
    if (subscribed_topics_.count(topic) > 0) {
      RCLCPP_ERROR(this->get_logger(), "Topic name [%s] is already used.",
                   topic.c_str());
      return false;
    }
  }
  return true;
}

slam_ros_msgs::msg::StatusResponse Node::TrajectoryStateToStatus(
    const int trajectory_id,
    const std::set<TrajectoryState>& valid_states) {
  const auto trajectory_states = map_builder_bridge_.GetTrajectoryStates();
  slam_ros_msgs::msg::StatusResponse status_response;

  const auto it = trajectory_states.find(trajectory_id);
  if (it == trajectory_states.end()) {
    status_response.message =
        absl::StrCat("Trajectory ", trajectory_id, " doesn't exist.");
    status_response.code = slam_ros_msgs::msg::StatusCode::NOT_FOUND;
    return status_response;
  }

  status_response.message =
      absl::StrCat("Trajectory ", trajectory_id, " is in '",
                   TrajectoryStateToString(it->second), "' state.");
  status_response.code =
      valid_states.count(it->second)
          ? slam_ros_msgs::msg::StatusCode::OK
          : slam_ros_msgs::msg::StatusCode::INVALID_ARGUMENT;
  return status_response;
}

slam_ros_msgs::msg::StatusResponse Node::FinishTrajectoryUnderLock(
    const int trajectory_id) {
  RCLCPP_WARN(this->get_logger(),
              "FinishTrajectory: Will finish trajectory %d UnderLock",
              trajectory_id);
  slam_ros_msgs::msg::StatusResponse status_response;
  if (trajectories_scheduled_for_finish_.count(trajectory_id)) {
    status_response.message = absl::StrCat("Trajectory ", trajectory_id,
                                           " already pending to finish.");
    status_response.code = slam_ros_msgs::msg::StatusCode::OK;
    RCLCPP_WARN(this->get_logger(), "%s", status_response.message.c_str());
    return status_response;
  }

  // First, check if we can actually finish the trajectory.
  status_response = TrajectoryStateToStatus(
      trajectory_id, {TrajectoryState::ACTIVE} /* valid states */);
  if (status_response.code != slam_ros_msgs::msg::StatusCode::OK) {
    RCLCPP_ERROR(this->get_logger(), "Can't finish trajectory: %s",
                 status_response.message.c_str());
    return status_response;
  }

  // Shutdown the subscribers of this trajectory.
  // A valid case with no subscribers is e.g. if we just visualize states.
  if (subscribers_.count(trajectory_id)) {
    for (auto& entry : subscribers_[trajectory_id]) {
      // In ROS2, subscriptions are managed by shared_ptr.
      // Resetting the pointer effectively "shuts down" the subscription.
      entry.subscriber.reset();
      subscribed_topics_.erase(entry.topic);
      RCLCPP_WARN(this->get_logger(), "Shutdown the subscriber of [%s]",
                  entry.topic.c_str());
    }
    CHECK_EQ(subscribers_.erase(trajectory_id), 1);
  }

  if (sensor_samplers_.count(trajectory_id)) {
    sensor_samplers_.erase(trajectory_id);
  }

  map_builder_bridge_.FinishTrajectory(trajectory_id);
  ResetSoftwareException();
  trajectories_scheduled_for_finish_.emplace(trajectory_id);
  status_response.message =
      absl::StrCat("Finished trajectory ", trajectory_id, ".");
  status_response.code = slam_ros_msgs::msg::StatusCode::OK;

  return status_response;
}

bool Node::loadRTYAML(const std::string& file_name) {
  try {
    YAML::Node loaded = YAML::LoadFile(file_name);
    if (!loaded) {
      RCLCPP_INFO(this->get_logger(),
                  "Read RTYAML Failed. data is not yaml format...");
      return false;
    }
    double origin_tx = loaded["translation"].as<std::vector<double>>().at(0);
    double origin_ty = loaded["translation"].as<std::vector<double>>().at(1);
    double origin_tz = loaded["translation"].as<std::vector<double>>().at(2);

    double origin_rw = loaded["rotation"].as<std::vector<double>>().at(0);
    double origin_rx = loaded["rotation"].as<std::vector<double>>().at(1);
    double origin_ry = loaded["rotation"].as<std::vector<double>>().at(2);
    double origin_rz = loaded["rotation"].as<std::vector<double>>().at(3);

    const Eigen::Vector3d transtation(origin_tx, origin_ty, origin_tz);
    const Eigen::Quaterniond rotation(origin_rw, origin_rx, origin_ry,
                                      origin_rz);

    map_init_pose_from_yaml =
        ::slam::transform::Rigid3d(transtation, rotation);
  } catch (const YAML::Exception& e) {
    RCLCPP_INFO(this->get_logger(), "Read %s error is %s", file_name.c_str(),
                e.what());
    return false;
  }

  return true;
}

void Node::HandleStartTrajectory(
    const std::shared_ptr<slam_ros_msgs::srv::StartTrajectory::Request> request,
    std::shared_ptr<slam_ros_msgs::srv::StartTrajectory::Response> response) {
  RCLCPP_WARN(this->get_logger(),
              "StartTrajectory: StartTrajectory service was called!");
  TrajectoryOptions trajectory_options;
  std::tie(std::ignore, trajectory_options) = LoadOptions(
      request->configuration_directory, request->configuration_basename);
  const std::string config_file =
      request->configuration_directory + "/" + request->configuration_basename;
  RCLCPP_WARN(this->get_logger(), "StartTrajectory: StartTrajectory with %s",
              config_file.c_str());
  if (request->use_initial_pose) {
    loadRTYAML(current_localization_map_ + "_rt.yaml");
    slam::transform::Rigid3d inversed_pose = map_init_pose_from_yaml.inverse();
    const auto pose = inversed_pose * ToRigid3d(request->initial_pose);
    RCLCPP_WARN(this->get_logger(), "StartTrajectory: set init pose as %s",
                slam::transform::Project2D(pose).DebugString().c_str());
    if (!pose.IsValid()) {
      response->status.message =
          "Invalid pose argument. Orientation quaternion must be normalized.";
      RCLCPP_ERROR(this->get_logger(), "%s",
                   response->status.message.c_str());
      response->status.code =
          slam_ros_msgs::msg::StatusCode::INVALID_ARGUMENT;
      return;
    }

    response->status = TrajectoryStateToStatus(
        request->relative_to_trajectory_id,
        {TrajectoryState::ACTIVE, TrajectoryState::FROZEN,
         TrajectoryState::FINISHED});
    if (response->status.code != slam_ros_msgs::msg::StatusCode::OK) {
      RCLCPP_ERROR(this->get_logger(),
                   "Can't start a trajectory with initial pose: %s",
                   response->status.message.c_str());
      return;
    }

    ::slam::mapping::proto::InitialTrajectoryPose initial_trajectory_pose;
    initial_trajectory_pose.set_to_trajectory_id(
        request->relative_to_trajectory_id);
    *initial_trajectory_pose.mutable_relative_pose() =
        slam::transform::ToProto(pose);
    initial_trajectory_pose.set_timestamp(
        slam::common::ToUniversal(::slam_ros::FromRos(rclcpp::Time(0))));
    *trajectory_options.trajectory_builder_options
         .mutable_initial_trajectory_pose() = initial_trajectory_pose;
  }

  if (!ValidateTrajectoryOptions(trajectory_options)) {
    response->status.message = "Invalid trajectory options.";
    RCLCPP_ERROR(this->get_logger(), "%s", response->status.message.c_str());
    response->status.code = slam_ros_msgs::msg::StatusCode::INVALID_ARGUMENT;
  } else if (!ValidateTopicNames(trajectory_options)) {
    response->status.message = "Topics are already used by another trajectory.";
    RCLCPP_ERROR(this->get_logger(), "%s", response->status.message.c_str());
    response->status.code = slam_ros_msgs::msg::StatusCode::INVALID_ARGUMENT;
  } else {
    response->status.message = "Success.";
    response->trajectory_id = AddTrajectory(trajectory_options);
    response->status.code = slam_ros_msgs::msg::StatusCode::OK;
  }
}

void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
  RCLCPP_WARN(this->get_logger(),
              "StartTrajectory: StartTrajectoryWithDefaultTopics!");
  absl::MutexLock lock(&mutex_);
  CHECK(ValidateTrajectoryOptions(options));
  // trajectory_options_ = options;
  AddTrajectory(options);
}

std::vector<std::set<slam::mapping::TrajectoryBuilderInterface::SensorId>>
Node::ComputeDefaultSensorIdsForMultipleBags(
    const std::vector<TrajectoryOptions>& bags_options) const {
  using SensorId = slam::mapping::TrajectoryBuilderInterface::SensorId;
  std::vector<std::set<SensorId>> bags_sensor_ids;
  for (size_t i = 0; i < bags_options.size(); ++i) {
    std::string prefix;
    if (bags_options.size() > 1) {
      prefix = "bag_" + std::to_string(i + 1) + "_";
    }
    std::set<SensorId> unique_sensor_ids;
    for (const auto& sensor_id :
         ComputeExpectedSensorIds(bags_options.at(i))) {
      unique_sensor_ids.insert(
          SensorId{sensor_id.type, prefix + sensor_id.id});
    }
    bags_sensor_ids.push_back(unique_sensor_ids);
  }
  return bags_sensor_ids;
}

int Node::AddOfflineTrajectory(
    const std::set<slam::mapping::TrajectoryBuilderInterface::SensorId>&
        expected_sensor_ids,
    const TrajectoryOptions& options) {
  absl::MutexLock lock(&mutex_);
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  AddSensorSamplers(trajectory_id, options);

  trajectory_options_ = options;
  return trajectory_id;
}

void Node::HandleGetTrajectoryStates(
    const std::shared_ptr<slam_ros_msgs::srv::GetTrajectoryStates::Request>
        request,
    std::shared_ptr<slam_ros_msgs::srv::GetTrajectoryStates::Response>
        response) {
  (void)request;  // Unused parameter
  using TrajectoryState = ::slam::mapping::PoseGraphInterface::TrajectoryState;
  absl::MutexLock lock(&mutex_);
  response->status.code = ::slam_ros_msgs::msg::StatusCode::OK;
  response->trajectory_states.header.stamp = this->now();
  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
    response->trajectory_states.trajectory_id.push_back(entry.first);
    switch (entry.second) {
      case TrajectoryState::ACTIVE:
        response->trajectory_states.trajectory_state.push_back(
            ::slam_ros_msgs::msg::TrajectoryStates::ACTIVE);
        break;
      case TrajectoryState::FINISHED:
        response->trajectory_states.trajectory_state.push_back(
            ::slam_ros_msgs::msg::TrajectoryStates::FINISHED);
        break;
      case TrajectoryState::FROZEN:
        response->trajectory_states.trajectory_state.push_back(
            ::slam_ros_msgs::msg::TrajectoryStates::FROZEN);
        break;
      case TrajectoryState::DELETED:
        response->trajectory_states.trajectory_state.push_back(
            ::slam_ros_msgs::msg::TrajectoryStates::DELETED);
        break;
    }
  }
}

void Node::HandleFinishTrajectory(
    const std::shared_ptr<slam_ros_msgs::srv::FinishTrajectory::Request>
        request,
    std::shared_ptr<slam_ros_msgs::srv::FinishTrajectory::Response> response) {
  RCLCPP_WARN(this->get_logger(),
              "FinishTrajectory: FinishTrajectory service was called!");
  absl::MutexLock lock(&mutex_);
  response->status = FinishTrajectoryUnderLock(request->trajectory_id);
}

void Node::HandleOnlyUseOdomPredictPoseFlag(
    const std::shared_ptr<slam_ros_msgs::srv::OnlyUseOdomPredictPose::Request>
        request,
    std::shared_ptr<slam_ros_msgs::srv::OnlyUseOdomPredictPose::Response>
        response) {
  RCLCPP_WARN(
      this->get_logger(),
      "Service: OnlyUseOdomPredictPose service was called!");
  absl::MutexLock lock(&mutex_);
  bool status = SetOnlyUseOdomPredictPoseFlag(request->useodom_predict_pose);
  // Assuming the service response requires a status message and code.
  // The original code returned a bool, which is not a valid service response.
  response->status = status;
}

void Node::HandleLogSlamData(
    const std::shared_ptr<slam_ros_msgs::srv::LogSlamData::Request> request,
    std::shared_ptr<slam_ros_msgs::srv::LogSlamData::Response> response) {
  if (request->logstatus != 1) {
    response->status = false;
    return;
  }
  response->status = true;

  absl::MutexLock lock(&mutex_);
  log_sensor_data_ = true;
}

bool Node::SetOnlyUseOdomPredictPoseFlag(bool flag) {
  map_builder_bridge_.SetOnlyUseOdomPredictPoseFlag(flag);
  return flag;
}

void Node::HandleWriteState(
    const std::shared_ptr<slam_ros_msgs::srv::WriteState::Request> request,
    std::shared_ptr<slam_ros_msgs::srv::WriteState::Response> response) {
  RCLCPP_WARN(this->get_logger(), "Service: WriteState service was called!");
  absl::MutexLock lock(&mutex_);
  if (map_builder_bridge_.SerializeState(request->filename,
                                         true /*cover_old_map*/,
                                         request->include_unfinished_submaps)) {
    response->status.code = slam_ros_msgs::msg::StatusCode::OK;
    response->status.message =
        absl::StrCat("State written to '", request->filename, "'.");
  } else {
    response->status.code = slam_ros_msgs::msg::StatusCode::INVALID_ARGUMENT;
    response->status.message =
        absl::StrCat("Failed to write '", request->filename, "'.");
  }
}

void Node::HandleReadMetrics(
    const std::shared_ptr<slam_ros_msgs::srv::ReadMetrics::Request> request,
    std::shared_ptr<slam_ros_msgs::srv::ReadMetrics::Response> response) {
  (void)request;  // Unused parameter
  absl::MutexLock lock(&mutex_);
  response->timestamp = this->now();
  if (!metrics_registry_) {
    response->status.code = slam_ros_msgs::msg::StatusCode::UNAVAILABLE;
    response->status.message =
        "Collection of runtime metrics is not activated.";
    return;
  }
  metrics_registry_->ReadMetrics(response.get());
  response->status.code = slam_ros_msgs::msg::StatusCode::OK;
  response->status.message = "Successfully read metrics.";
}

void Node::FinishAllTrajectories() {
  RCLCPP_WARN(this->get_logger(),
              "Service: FinishAllTrajectories service was called!");
  absl::MutexLock lock(&mutex_);
  for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
    if (entry.second == TrajectoryState::ACTIVE) {
      const int trajectory_id = entry.first;
      CHECK_EQ(FinishTrajectoryUnderLock(trajectory_id).code,
               slam_ros_msgs::msg::StatusCode::OK);
    }
  }
}

bool Node::FinishTrajectory(const int trajectory_id) {
  RCLCPP_WARN(this->get_logger(),
              "FinishTrajectory: Will Finish trajectory: %d", trajectory_id);
  absl::MutexLock lock(&mutex_);
  return FinishTrajectoryUnderLock(trajectory_id).code ==
         slam_ros_msgs::msg::StatusCode::OK;
}

void Node::RunFinalOptimization() {
  {
    for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) {
      const int trajectory_id = entry.first;
      if (entry.second == TrajectoryState::ACTIVE) {
        RCLCPP_WARN(this->get_logger(),
                    "Can't run final optimization if there are one or more "
                    "active trajectories. Trying to finish trajectory "
                    "with ID %d now.",
                    trajectory_id);
        CHECK(FinishTrajectory(trajectory_id))
            << "Failed to finish trajectory with ID "
            << std::to_string(trajectory_id) << ".";
      }
    }
  }
  // Assuming we are not adding new data anymore, the final optimization
  // can be performed without holding the mutex.
  map_builder_bridge_.RunFinalOptimization();
}

void Node::HandleOdometryMessage(
    const int trajectory_id, const std::string& sensor_id,
    const agv_msgs::msg::OdometryLite::ConstSharedPtr msg) {
  absl::MutexLock lock(&mutex_);
  if (slam_cmd_ == 0) return;
  if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
    return;
  }

  this->get_parameter("/AGVParam/agv_navigation_type", is_laser_nav);
  sensor_data_cache_->AddOdomData(std::const_pointer_cast<agv_msgs::msg::OdometryLite>(msg));

  if (is_odom_initialized_) {
    const double delta_t = rclcpp::Time(msg->header.stamp).seconds() -
                           rclcpp::Time(last_odom_msg_.header.stamp).seconds();
    if (delta_t > 1.0) {
      is_odom_ok_ = false;
      RCLCPP_WARN(this->get_logger(), "Odom data timestamp error, delta_t: %f",
                  delta_t);
    } else if (delta_t <= 0) {
      is_odom_ok_ = false;
      RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 20000,
                           "Odom data timestamp error, delta_t: %f", delta_t);
    } else {
      is_odom_ok_ = true;
    }
    if (delta_t <= 0) {
      return;
    }
  } else {
    is_odom_initialized_ = true;
  }
  odometry_data_time_last_ = this->now().seconds();
  last_odom_msg_ = *msg;

  RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
                       "cur NavType is %d", is_laser_nav);
  if (trajectory_options_.use_odometry) {
    slam_ros_msgs::msg::RobotStatus robot_status;
    robot_status.slam_type.data = static_cast<int>(slam_type_);
    robot_status.localization_map = current_localization_map_;
    robot_status.laser_to_base = laser_to_base_;
    if (is_localization_ok_ && is_laser_ok_ && is_odom_ok_ && is_imu_ok_) {
      absl::MutexLock lock(&pose_publish_mutex_);
      const Rigid3d T_odom_base = ToRigid3d(msg->pose);
      robot_pose_now_ = map_init_pose_from_yaml * T_map_odom_ * T_odom_base;

      const geometry_msgs::msg::PointStamped robot_stamped_pose =
          ToPointStampedPose(robot_pose_now_, msg->header.stamp);
      sensor_data_cache_->AddRobotPose(std::make_shared<geometry_msgs::msg::PointStamped>(robot_stamped_pose));
      // RCLCPP_INFO(this->get_logger(), "=======HandleOdometryMessage========");
      robot_pose_pub_->publish(robot_stamped_pose);

      robot_pose_with_confidence_pub_->publish(
          ToPoseStamped(robot_pose_now_, robot_stamped_pose.header.stamp));

      robot_status.localization_state.data = 1;
      robot_status.localization_pose = robot_stamped_pose;
      robot_status_->publish(robot_status);

      std::vector<geometry_msgs::msg::TransformStamped> stamped_transforms;
      geometry_msgs::msg::TransformStamped stamped_transform;
      stamped_transform.header.stamp = msg->header.stamp;

      stamped_transform.header.frame_id = node_options_.map_frame;
      stamped_transform.child_frame_id = trajectory_options_.odom_frame;
      stamped_transform.transform = ToGeometryMsgTransform(T_map_odom_);
      stamped_transforms.push_back(stamped_transform);

      stamped_transform.header.frame_id = trajectory_options_.odom_frame;
      stamped_transform.child_frame_id = trajectory_options_.published_frame;
      stamped_transform.transform = ToGeometryMsgTransform(T_odom_base);
      stamped_transforms.push_back(stamped_transform);

      tf_broadcaster_->sendTransform(stamped_transforms);

      agv_msgs::msg::PoseWithConfidence PoseWithConfidence_msg =
          ToTrackedPoseWithConfidence(robot_pose_now_, this->now());

      agv_msgs::msg::ConfidenceLevel Confidence_score;
      Confidence_score.score = PoseWithConfidence_msg.score;

      cur_confidence_pub_->publish(Confidence_score);
      if (is_laser_nav == 0) {
        pose_with_confidence_pub_->publish(PoseWithConfidence_msg);
      }
	    // add after publish
      {
        absl::MutexLock lock(&data_mutex_);
        last_robot_stamped_pose_ = robot_stamped_pose;  // ´æ´¢×îÐÂÎ»×Ë
        last_PoseWithConfidence_msg_ =
            PoseWithConfidence_msg;  // ´æ´¢ÖÃÐÅ¶ÈÏûÏ¢
      }
      if (is_laser_nav == 4) {
        fusion_laserpose_pub_->publish(PoseWithConfidence_msg);
      }

    } else {
      absl::MutexLock lock(&pose_publish_mutex_);
      const Rigid3d T_odom_base = ToRigid3d(msg->pose);
      robot_pose_now_ = map_init_pose_from_yaml * T_map_odom_ * T_odom_base;

      robot_status.localization_state.data = 0;
      robot_status.localization_pose =
          ToPointStampedPose(robot_pose_now_, msg->header.stamp);
      robot_status_->publish(robot_status);

      agv_msgs::msg::PoseWithConfidence PoseWithConfidence_msg =
          ToTrackedPoseWithConfidence(robot_pose_now_, this->now());

      agv_msgs::msg::ConfidenceLevel Confidence_score;
      Confidence_score.score = PoseWithConfidence_msg.score;

      cur_confidence_pub_->publish(Confidence_score);
      if (is_laser_nav == 0) {
        pose_with_confidence_pub_->publish(PoseWithConfidence_msg);
      }
      if (is_laser_nav == 4) {
        fusion_laserpose_pub_->publish(PoseWithConfidence_msg);
      }
    }
  }

  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
}

void Node::HandleNavSatFixMessage(
    const int trajectory_id, const std::string& sensor_id,
    const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) {
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleNavSatFixMessage(sensor_id, msg);
}

void Node::HandleLandmarkMessage(
    const int trajectory_id, const std::string& sensor_id,
    const slam_ros_msgs::msg::LandmarkList::ConstSharedPtr msg) {
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLandmarkMessage(sensor_id, msg);
}

void Node::HandleQRCodeMessage(
    const int trajectory_id, const std::string& sensor_id,
    const slam_ros_msgs::msg::QRCode::ConstSharedPtr msg) {
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).qr_code_sampler.Pulse()) {
    return;
  }

  sensor_data_cache_->AddQRCode(std::const_pointer_cast<slam_ros_msgs::msg::QRCode>(msg));

  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleQRCodeMessage(sensor_id, *msg,
                            trajectory_options_.qr_code_translation_weight,
                            trajectory_options_.qr_code_rotation_weight);
}

void Node::HandleImuMessage(const int trajectory_id, const std::string& sensor_id,
                            const sensor_msgs::msg::Imu::ConstSharedPtr msg) {
  absl::MutexLock lock(&mutex_);
  if (slam_cmd_ == 0) return;
  if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
    return;
  }
  sensor_data_cache_->AddImuData(std::const_pointer_cast<sensor_msgs::msg::Imu>(msg));

  if (is_imu_initialized_) {
    const double delta_t = rclcpp::Time(msg->header.stamp).seconds() -
                           rclcpp::Time(last_imu_msg_.header.stamp).seconds();
    if (delta_t > 1.0) {
      is_imu_ok_ = false;
      RCLCPP_WARN(this->get_logger(), "Imu data timestamp error, delta_t: %f",
                  delta_t);
    } else if (delta_t <= 0) {
      is_imu_ok_ = false;
      RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 10000,
                           "Imu data timestamp error, delta_t: %f", delta_t);
    } else {
      is_imu_ok_ = true;
    }
    if (delta_t <= 0) {
      return;
    }
  } else {
    is_imu_initialized_ = true;
  }
  imu_data_time_last_ = this->now().seconds();
  last_imu_msg_ = *msg;

  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

// Add load ignore regions(onle once)
void Node::load_ignore_regions_once() {
  if (ignore_regions_loaded_) {
    RCLCPP_INFO(this->get_logger(), "loading ignore_regions_loaded_ success");
    return;
  }

  RCLCPP_INFO(this->get_logger(), "loading ignore_regions_loaded_ ing");

  ignore_regions_.clear();
  ignore_region_config_path_.clear();

  if (ignore_region_config_path_.empty()) {  // 使用empty()替代size()==0
    // 1. 定义YAML文件路径
    const std::string config_path = kCurrentInitMapID;
    std::string agv_map_id;
    try {
      YAML::Node config = YAML::LoadFile(config_path);
      if (!config["agv_map_name"]) {
        RCLCPP_ERROR(this->get_logger(), "Key 'agv_map_name' missing in YAML");
        return;
      }
      agv_map_id = config["agv_map_name"].as<std::string>();
      ignore_region_config_path_ =
          kMapPath + "../" + agv_map_id + "_region.json";
    } catch (const YAML::Exception& e) {
      RCLCPP_ERROR(this->get_logger(), "YAML error: %s", e.what());
      return;
    }
  }

  if (!ignore_region_config_path_.empty()) {
    std::ifstream file(ignore_region_config_path_);
    if (!file.is_open()) {
      RCLCPP_WARN(this->get_logger(), "faild: %s", ignore_region_config_path_.c_str());
      return;
    }
    try {
      Json::Value root;
      file >> root;

      const Json::Value regions = root["region_data_buffer"];
      for (const auto& region : regions) {
        if (region["region_category"].asInt() == 0 &&
            region["region_shape"].asInt() == 0) {
          const Json::Value buffer = region["data_buffer"];
          if (buffer.size() == 4) {
            double x1 = buffer[0].asDouble();
            double y1 = buffer[1].asDouble();
            double x2 = buffer[2].asDouble();
            double y2 = buffer[3].asDouble();

            // 标准化坐标
            std::vector<double> normalized{std::min(x1, x2), std::max(x1, x2),
                                           std::min(y1, y2), std::max(y1, y2)};
            ignore_regions_.push_back(normalized);
          }
        }
      }
    } catch (const Json::Exception& e) {
      RCLCPP_ERROR(this->get_logger(), "JSON parse error: %s", e.what());
    }
    RCLCPP_INFO(this->get_logger(), "successe %zu regions", ignore_regions_.size());

    if (!ignore_regions_.empty()) {
      ignore_regions_loaded_ = true;
    }
  }
}

void Node::HandleLaserScanMessage(
    const int trajectory_id, const std::string& sensor_id,
    const sensor_msgs::msg::LaserScan::ConstSharedPtr msg) {
  sensor_msgs::msg::LaserScan::SharedPtr modified_msg =
      std::make_shared<sensor_msgs::msg::LaserScan>(*msg);

  RCLCPP_INFO(
      this->get_logger(),
      "=================start_handleLaserScanMessage=====================");

  RCLCPP_INFO_STREAM(this->get_logger(), 
      " is_localization_ok_ " << is_localization_ok_ << " slam_type_ " << slam_type_
      << " current_agv_mode_ " << current_agv_mode_);
  
  //  1定位 2slam模式：定位 建图 3车辆模式：手动 自动
  //=======================================================处理modified_msg======================================================
  if (is_localization_ok_ == 1 && slam_type_ == 1) {
    agv_msgs::msg::PoseWithConfidence current_PoseWithConfidence_msg;

    {
      absl::MutexLock lock(&data_mutex_);
      current_PoseWithConfidence_msg = last_PoseWithConfidence_msg_;
    }
 
    // 仅在首次进入时查询变换
    if (!first_transform_obtained_) {
      try {
        base_to_laser_transform_ = tf_buffer_->lookupTransform(
            "base_link", "laser_link_1", msg->header.stamp,
            rclcpp::Duration::from_seconds(0.1));  // ROS2 Duration API变化
        first_transform_obtained_ = true;
        RCLCPP_INFO(this->get_logger(), "first base_to_laser_transform_ success!!!");
      } catch (const tf2::TransformException& ex) {
        RCLCPP_WARN(this->get_logger(), "first base_to_laser_transform_ failed!!!: %s", ex.what());
        return;
      }
    }
   
    const auto& transform = base_to_laser_transform_;

    // load_ignore_regions_once();

    if (last_current_agv_mode_ == 0 && current_agv_mode_ == 3) {
      ignore_regions_loaded_ = false;
    }

    last_current_agv_mode_ = current_agv_mode_;

    RCLCPP_INFO_STREAM(this->get_logger(), 
        " first_transform_obtained_ " << first_transform_obtained_
        << " ignore_regions_.empty() " << ignore_regions_.empty());

    RCLCPP_WARN(this->get_logger(), "Current ignore_region_config_path_ name: %s",
               ignore_region_config_path_.c_str());

    if (first_transform_obtained_ && !ignore_regions_.empty()) {
      if (!ignore_regions_.empty()) {
        RCLCPP_INFO(this->get_logger(), "Loaded ignore regions:");
        for (size_t i = 0; i < ignore_regions_.size(); ++i) {
          const auto& region = ignore_regions_[i];
          RCLCPP_INFO_STREAM(this->get_logger(), 
              "Region " << i << ": [" << region[0] << ", " << region[1] 
              << "] -> [" << region[2] << ", " << region[3] << "]");
        }
      } else {
        RCLCPP_WARN(this->get_logger(), "No ignore regions loaded");
      }

      Eigen::Vector3d base_to_laser_translation(
          transform.transform.translation.x, 
          transform.transform.translation.y,
          0);

      Eigen::Quaterniond base_to_laser_rotation(
          transform.transform.rotation.w, 
          transform.transform.rotation.x,
          transform.transform.rotation.y,
          transform.transform.rotation.z);

      Eigen::Affine3d base_to_laser_transform = Eigen::Affine3d::Identity();
      base_to_laser_transform.translate(base_to_laser_translation)
          .rotate(base_to_laser_rotation);

      Eigen::Vector3d map_to_base_translation(
          current_PoseWithConfidence_msg.pose.pose.position.x,
          current_PoseWithConfidence_msg.pose.pose.position.y, 0);
      Eigen::Quaterniond map_to_base_rotation(
          current_PoseWithConfidence_msg.pose.pose.orientation.w,
          current_PoseWithConfidence_msg.pose.pose.orientation.x,
          current_PoseWithConfidence_msg.pose.pose.orientation.y,
          current_PoseWithConfidence_msg.pose.pose.orientation.z);

      // 构建齐次变换矩阵
      Eigen::Affine3d map_to_base_transform = Eigen::Affine3d::Identity();
      map_to_base_transform.translate(map_to_base_translation)
          .rotate(map_to_base_rotation);

      for (size_t i = 0; i < modified_msg->ranges.size(); ++i) {
        float angle = modified_msg->angle_min + i * modified_msg->angle_increment;
        float x_laser = modified_msg->ranges[i] * cos(angle);
        float y_laser = modified_msg->ranges[i] * sin(angle);

        geometry_msgs::msg::PointStamped point_laser, point_base;
        point_laser.header = modified_msg->header;
        point_laser.header.frame_id = "laser_link_1";
        point_laser.point.x = x_laser;
        point_laser.point.y = y_laser;

        Eigen::Vector3d vec_point_laser(x_laser, y_laser, 0);
        Eigen::Vector3d vec_point_base = base_to_laser_transform * vec_point_laser;
        Eigen::Vector3d vec_point_map = map_to_base_transform * vec_point_base;

        if (!ignore_regions_.empty()) {
          for (size_t j = 0; j < ignore_regions_.size(); ++j) {
            const auto& region = ignore_regions_[j];
            if (region[0] < vec_point_map.x() && vec_point_map.x() < region[1] &&
                region[2] < vec_point_map.y() && vec_point_map.y() < region[3]) {
              modified_msg->ranges[i] = 0;
            }
          }
        } else {
          RCLCPP_WARN(this->get_logger(), "No ignore regions loaded");
        }
      }
    }
  } else {
    RCLCPP_INFO_STREAM(this->get_logger(), "No deal modified_msg ");
  }

  // 未处理modified_msg部分
  absl::MutexLock lock(&mutex_);

  if (slam_cmd_ == 0) return;
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return;
  }

  sensor_data_cache_->AddLaserData(modified_msg);

  if (is_scan_initialized_) {
    const double delta_t =
        rclcpp::Time(msg->header.stamp).seconds() -
        rclcpp::Time(last_laser_msg_.header.stamp).seconds();
    if (delta_t > 1.0) {
      is_laser_data_ok_ = false;
      RCLCPP_WARN(this->get_logger(), "Laser data timestamp error, delta_t: %f", delta_t);
    } else if (delta_t <= 0) {
      is_laser_data_ok_ = false;
      RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000,
                          "Laser data timestamp error, delta_t: %f", delta_t);
    } else {
      is_laser_data_ok_ = true;
    }
    if (delta_t <= 0) {
      return;
    }
  } else {
    is_scan_initialized_ = true;
  }
  laser_data_time_last_ = this->now().seconds();
  last_laser_msg_ = *modified_msg;

  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, modified_msg);
  is_slam_laser_data_ok_ = map_builder_bridge_.QueryFontLaserDataStatus();
}

void Node::HandleMultiEchoLaserScanMessage(
    const int trajectory_id, const std::string& sensor_id,
    const sensor_msgs::msg::MultiEchoLaserScan::ConstSharedPtr msg) {
  absl::MutexLock lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return;
  }
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleMultiEchoLaserScanMessage(sensor_id, msg);
}

void Node::HandlePointCloud2Message(
    const int trajectory_id, const std::string& sensor_id,
    const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) {
  absl::MutexLock lock(&mutex_);
  if (slam_cmd_ == 0) return;
  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
    return;
  }

  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandlePointCloud2Message(sensor_id, msg);
}

void Node::SerializeState(const std::string& filename,
                          const bool cover_old_map,
                          const bool include_unfinished_submaps) {
  absl::MutexLock lock(&mutex_);
  slam_ros_msgs::msg::MapdataProgress msg;
  const std::string suffix = ".pbstream";
  const int map_name_len = filename.size() - suffix.size();
  const std::string map_id = filename.substr(0, map_name_len);
  const std::string map_path = std::string(getenv("HOME")) + "/map/";
  current_save_map_id_ = map_id.substr(map_path.size(), map_id.size());
  msg.current_mapfile_name = current_save_map_id_;
  msg.status = true;
  msg.current_progress = 20;
  setSavemapProgress(msg);

  if (!map_builder_bridge_.SerializeState(filename, cover_old_map,
                                          include_unfinished_submaps)) {
    msg.status = false;
    msg.current_progress = 100;
    msg.error = "file not permission.";
    setSavemapProgress(msg);
    RCLCPP_ERROR(this->get_logger(), "Could not write state.");
    return;
  }

  msg.current_progress = 60;
  setSavemapProgress(msg);
  // compress map
  const std::string map_zip = map_id + ".tar.gz";
  const std::string map_png = map_id + ".png";
  const std::string map_yaml = map_id + ".yaml";
  const std::string map_rtyaml = map_id + "_rt.yaml";
  const std::string map_param =
      std::string(getenv("HOME")) + "/map/config/";
  const std::string cmd = "tar -zcvf " + map_zip + " " + filename + " " +
                          map_png + " " + map_yaml + " " + map_rtyaml + " " +
                          map_param;

  int ret = system(cmd.c_str());

  RCLCPP_INFO(this->get_logger(), "ret is %d", ret);
  if (ret == -1) {
    msg.current_progress = 100;
    msg.status = false;
    msg.error = "compress map file failed.";
    setSavemapProgress(msg);
    RCLCPP_ERROR(this->get_logger(), "cannot compress map.");
    return;
  }

  msg.current_progress = 100;
  msg.status = true;
  setSavemapProgress(msg);

  const std::string chmod_cmd =
      "echo 'vensin'| sudo -S chmod -R 777 " + map_path;

  int chmod_ret = system(chmod_cmd.c_str());

  RCLCPP_INFO(this->get_logger(), "chmod ret is %d", chmod_ret);
  if (chmod_ret == -1 || chmod_ret == 256) {
    msg.current_progress = 100;
    msg.status = false;
    msg.error = "system permission failed.";
    setSavemapProgress(msg);
    RCLCPP_ERROR(this->get_logger(), "cur map permission is wrong.");
    return;
  }
}

void Node::PbstreamToPgm(const std::string& pbstream_filename,
                         const std::string& map_filename,
                         const double resolution) const {
  ::slam::io::ProtoStreamReader reader(pbstream_filename);
  ::slam::io::ProtoStreamDeserializer deserializer(&reader);
  RCLCPP_INFO(this->get_logger(),
              "Loading submap slices from serialized data.");
  std::map<::slam::mapping::SubmapId, ::slam::io::SubmapSlice> submap_slices;
  ::slam::mapping::ValueConversionTables conversion_tables;
  ::slam::io::DeserializeAndFillSubmapSlices(&deserializer, &submap_slices,
                                             &conversion_tables);
  CHECK(reader.eof());

  RCLCPP_INFO(this->get_logger(),
              "Generating combined map image from submap slices.");
  auto result = ::slam::io::PaintSubmapSlices(submap_slices, resolution);

  ::slam::io::StreamFileWriter pgm_writer(map_filename + ".pgm");

  ::slam::io::Image image(std::move(result.surface));
  WritePgm(image, resolution, &pgm_writer);

  const Eigen::Vector2d origin(
      -result.origin.x() * resolution,
      (result.origin.y() - image.height()) * resolution);

  ::slam::io::StreamFileWriter yaml_writer(map_filename + ".yaml");
  WriteYaml(resolution, origin, pgm_writer.GetFilename(), &yaml_writer);
}

void Node::LoadState(const std::string& state_filename,
                     const bool load_frozen_state) {
  absl::MutexLock lock(&mutex_);
  map_builder_bridge_.LoadState(state_filename, load_frozen_state);

  const std::string suffix = ".pbstream";
  const int map_name_len = state_filename.size() - suffix.size();
  current_localization_map_ = state_filename.substr(0, map_name_len);
  CHECK_EQ(current_localization_map_ + suffix, state_filename);

  const std::string file_before = std::string(getenv("HOME")) + "/map/";
  current_ZoneTag_ =
      current_localization_map_.substr(file_before.size(), map_name_len);
}

void Node::SetSlamType(SlamType type) {
  absl::MutexLock lock(&mutex_);
  slam_type_ = type;
}

void Node::SetLasetToBaseTransform(const Rigid3d& T_laser_base) {
  absl::MutexLock lock(&mutex_);
  laser_to_base_ = ToGeometryMsgTransform(T_laser_base);
}

void Node::PublishPose(
    const slam::common::Time time, const slam::transform::Rigid3d T_map_odom,
    const Rigid3d odom_pose,
    const slam::mapping::ReflectorMatchResult reflector_match_result) {
  (void)time; // Unused parameter
  absl::MutexLock lock(&pose_publish_mutex_);
  if (is_localization_ok_ && is_laser_ok_ && is_odom_ok_ && is_imu_ok_) {
    if (node_options_.min_reflector_fusion_num >= 2 &&
        reflector_match_result.matched_num >=
            node_options_.min_reflector_fusion_num) {
      T_map_odom_ = reflector_match_result.robot_pose * odom_pose.inverse();
    } else {
      T_map_odom_ = T_map_odom;
    }

    if (reflector_match_result.matched_num >= 2) {
      slam_ros_msgs::msg::ReflectorLandmark reflector_result;
      reflector_result.reflector_num = reflector_match_result.matched_num;
      reflector_result.robot_pose = ToPointStampedPose(
          reflector_match_result.robot_pose,
          ToRos(reflector_match_result.time));
      reflector_landmark_pub_->publish(reflector_result);
    }
  }
}

geometry_msgs::msg::PointStamped Node::ToPointStampedPose(
    const Rigid3d& robot_pose, const rclcpp::Time& time) {
  geometry_msgs::msg::PointStamped robot_stamped_pose;
  robot_stamped_pose.header.frame_id = node_options_.map_frame;
  robot_stamped_pose.header.stamp = time;
  robot_stamped_pose.point.x = robot_pose.translation().x();
  robot_stamped_pose.point.y = robot_pose.translation().y();
  robot_stamped_pose.point.z = slam::transform::GetYaw(robot_pose.rotation());
  return robot_stamped_pose;
}

slam_ros_msgs::msg::PoseWithConfidence Node::ToPoseWithConfidence(
    const Rigid3d& robot_pose, const rclcpp::Time& time) {
  slam_ros_msgs::msg::PoseWithConfidence pose_msg;
  const double match_score = map_builder_bridge_.GetMeanMatchScore();
  pose_msg.qr_init = false;
  pose_msg.score = match_score;
  pose_msg.header.frame_id = "map";
  // pose_msg.header.seq = robot_pose_seq_++; // seq is deprecated in ROS2
  pose_msg.header.stamp = time;
  pose_msg.pose.pose = ToGeometryMsgPose(robot_pose);
  return pose_msg;
}

geometry_msgs::msg::PoseStamped Node::ToPoseStamped(
    const Rigid3d& robot_pose, const rclcpp::Time& time) {
  geometry_msgs::msg::PoseStamped pose_msg;
  pose_msg.pose = ToGeometryMsgPose(robot_pose);
  pose_msg.header.frame_id = "map";
  // pose_msg.header.seq = robot_pose_seq_++; // seq is deprecated in ROS2
  pose_msg.header.stamp = time;
  return pose_msg;
}

agv_msgs::msg::PoseWithConfidence Node::ToTrackedPoseWithConfidence(
    const slam::transform::Rigid3d& robot_pose, const rclcpp::Time& time) {
  agv_msgs::msg::PoseWithConfidence pose_msg;
  pose_msg.header.frame_id = node_options_.map_frame;
  pose_msg.header.stamp = time;
  pose_msg.pose.pose = ToGeometryMsgPose(robot_pose);
  for (int i = 0; i < 2; ++i)
    for (int j = 0; j < 2; ++j) pose_msg.pose.covariance[6 * i + j] = 1.0;
  pose_msg.pose.covariance[6 * 5 + 5] = 1.0;
  pose_msg.score = map_builder_bridge_.GetMeanMatchScore() * 100;
  pose_msg.qr_init = true;
  pose_msg.zonetag = current_ZoneTag_;

  if (pose_msg.score >= 60) {
    pose_msg.score = pose_msg.score * 0.15 + 85.0;
  }

  if (pose_msg.score < 60 && pose_msg.score > 40) {
    pose_msg.score = pose_msg.score * 0.15 + 80.0;
  }

  if (!is_localization_ok_ || !is_odom_ok_ || !is_laser_data_ok_) {
    pose_msg.score = 0.0;
    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000,
                         "is_localization_ok_ is %d", is_localization_ok_);
    if (begin_start_device) {
      pose_msg.pose.pose.position.x = -9999.0;
      pose_msg.pose.pose.position.y = -9999.0;
    }
  } else {
    begin_start_device = false;
  }

  if (is_localization_ok_ && pose_msg.score == 0) {
    RCLCPP_WARN_THROTTLE(
        this->get_logger(), *this->get_clock(), 5000,
        "is_localization_ok_ is %d, meanMatchscore is %f",
        is_localization_ok_, pose_msg.score);
    pose_msg.score = 15;
  }
  return pose_msg;
}

void Node::MaybeWarnAboutTopicMismatch() {
  auto topics_and_types = this->get_topic_names_and_types();
  std::set<std::string> published_topics;
  std::stringstream published_topics_string;
  for (const auto& topic_and_type : topics_and_types) {
    published_topics.insert(topic_and_type.first);
    published_topics_string << topic_and_type.first << ", ";
  }

  bool print_topics = false;
  for (const auto& entry : subscribers_) {
    int trajectory_id = entry.first;
    for (const auto& subscriber : entry.second) {
      // In ROS2, topic names are usually fully-qualified.
      const std::string& resolved_topic = subscriber.topic;
      if (published_topics.count(resolved_topic) == 0) {
        RCLCPP_WARN(this->get_logger(),
                    "Expected topic \"%s\" (trajectory %d) but no "
                    "publisher is currently active.",
                    resolved_topic.c_str(), trajectory_id);
        print_topics = true;
      }
    }
  }
  if (print_topics) {
    RCLCPP_INFO(this->get_logger(), "Currently available topics are: %s",
                published_topics_string.str().c_str());
  }
}

void Node::ResetSoftwareException() {
  is_laser_data_ok_ = true;
  is_slam_laser_data_ok_ = true;
  is_localization_ok_ = false;
  is_laser_ok_ = true;
  is_odom_ok_ = true;
  is_imu_ok_ = true;

  slam_cmd_ = 1;
  robot_pose_seq_ = 0;
}

void Node::LogRobotPose() {
  bool log_pose = false;
  Rigid3d pose_now = Rigid3d::Identity();
  {
    absl::MutexLock lock(&mutex_);
    if (!is_localization_ok_) {
      return;
    }
    pose_now = map_init_pose_from_yaml.inverse() * robot_pose_now_;
  }

  if (saved_first_pose_) {
    const Rigid3d pose_diff = last_robot_pose_.inverse() * pose_now;
    if (pose_diff.translation().norm() > distance_traveled_threshold_ ||
        slam::common::NormalizeAngleDifference(GetAngle(pose_diff)) >
            angle_traveled_threshold_) {
      last_robot_pose_ = pose_now;
      log_pose = true;
    }
  } else {
    saved_first_pose_ = true;
    last_robot_pose_ = pose_now;
    log_pose = true;
  }

  if (log_pose && is_localization_ok_) {
    std::ofstream outfile;
    outfile.open(kCurrentPose, std::ios_base::out | std::ios_base::trunc);
    if (!outfile) {
      RCLCPP_ERROR(this->get_logger(), "Cannot open : %s", kCurrentPose.c_str());
      return;
    }
    std::stringstream sstream;
    sstream.setf(std::ios::fixed);
    sstream << this->now().seconds() << ";" << pose_now.translation().x() << ";"
            << pose_now.translation().y() << ";" << GetYaw(pose_now);
    outfile << sstream.str() << std::endl;
    outfile.close();
  }
}

void Node::ProcessLogSensorData() {
  while (true) {
    const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
      return (log_sensor_data_ || stop_sensor_data_cache_thread_);
    };

    {
      absl::MutexLock lock(&mutex_);
      mutex_.Await(absl::Condition(&predicate));
    }

    if (stop_sensor_data_cache_thread_) {
      break;
    }

    bool has_data_to_save = false;
    {
      absl::MutexLock lock(&mutex_);
      if (log_sensor_data_) {
        const float data_buffer_length =
            sensor_data_cache_->GetBufferMaxLengthInSec();
        if (data_buffer_length > 90) {
          has_data_to_save = log_sensor_data_;
          sensor_data_cache_->SwapSensorData();
        } else {
          RCLCPP_WARN(this->get_logger(),
                      "LOG: sensor data buffer too short: %f s!",
                      data_buffer_length);
        }
        log_sensor_data_ = false;
      }
    }

    if (has_data_to_save) {
      sensor_data_cache_->LogSensorData();
    } else {
      std::this_thread::sleep_for(std::chrono::seconds(1));
    }
  }
}

void Node::HandleControlSLAM(
    const std::shared_ptr<slam_ros_msgs::srv::ControlSam::Request> request,
    std::shared_ptr<slam_ros_msgs::srv::ControlSam::Response> response) {
  absl::MutexLock lock(&mutex_);
  slam_cmd_ = request->slam_cmd;
  response->status = true;
}

void Node::HandlePgmToLocalizationMap(
    const std::shared_ptr<slam_ros_msgs::srv::PgmToLocalizationMap::Request>
        request,
    std::shared_ptr<slam_ros_msgs::srv::PgmToLocalizationMap::Response>
        response) {
  absl::MutexLock lock(&mutex_);
  const std::string result_message =
      map_builder_bridge_.PgmToLocalizationMap(request->pgm_file_path);
  if (result_message == "Success") {
    std::vector<std::string> old_map;
    old_map.emplace_back(current_localization_map_ + ".pbstream");
    old_map.emplace_back(current_localization_map_ + ".pgm");
    old_map.emplace_back(current_localization_map_ + ".jpg");
    old_map.emplace_back(current_localization_map_ + ".yaml");
    RenameFileWithTime(old_map);
    if (map_builder_bridge_.SavePoseGraphAndMap(current_localization_map_)) {
      response->message = "Success";
      response->status = true;
    } else {
      response->message = "Save map failed";
      response->status = false;
    }
  } else if (result_message == "Empty pgm") {
    response->message = "EmptyPgm";
    response->status = false;
  } else {
    response->message = result_message;
    response->status = false;
  }
}

void Node::HandleLandMarkLists(
    const std::shared_ptr<slam_ros_msgs::srv::GetLandmarkLists::Request>
        request,
    std::shared_ptr<slam_ros_msgs::srv::GetLandmarkLists::Response> response) {
  (void)request;  // Unused parameter
  absl::MutexLock lock(&mutex_);
  visualization_msgs::msg::MarkerArray LandMarkLists =
      map_builder_bridge_.GetLandmarkPosesList(this->now());

  if (LandMarkLists.markers.empty()) return;

  for (const auto& landmark : LandMarkLists.markers) {
    if (landmark.type != visualization_msgs::msg::Marker::SPHERE) continue;

    const Rigid3d cur_lanmark_pose =
        map_init_pose_from_yaml * ToRigid3d(landmark.pose);
    response->scale = landmark.scale;
    response->colors.push_back(landmark.color);
    geometry_msgs::msg::PointStamped Point;
    Point.header = landmark.header;
    Point.point.x = cur_lanmark_pose.translation().x();
    Point.point.y = cur_lanmark_pose.translation().y();
    Point.point.z = landmark.id;
    response->landmark_pose.push_back(Point);
  }
}

void Node::TimedClearLog() {
  absl::MutexLock lock(&mutex_);
  // clear_log_->AutoRemoveOldLogFIle(); // Assuming 'clear_log_' is defined
}

void Node::PublishSlamError() {
  absl::MutexLock lock(&mutex_);
  rclcpp::Time stamptmp = this->now();

  // slam_event_msg.header.seq += 1; // 'seq' is deprecated
  slam_event_msg.header.stamp = stamptmp;
  slam_event_msg.header.frame_id = this->get_name();
  slam_event_msg.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
  slam_event_msg.type = agv_msgs::msg::AGVEvent::INFO;
  slam_event_msg.action = agv_msgs::msg::AGVEvent::NONE;
  slam_event_msg.param1 = is_scan_initialized_;
  slam_event_msg.param2 = is_laser_data_ok_;
  slam_event_msg.param3 = is_slam_laser_data_ok_;
  slam_event_msg.param4 = is_odom_ok_;

  if (!is_scan_initialized_) {
    slam_event_msg.description = "laser is not initialized!";
    slam_event_msg.eventtime = stamptmp.seconds();
    slam_event_msg.ecode = agv_msgs::msg::AGVEvent::SLAM_LIDAR_DATA_ERROR;
    slam_event_msg.action = agv_msgs::msg::AGVEvent::DELAYSTOP;
    slam_event_msg.type = agv_msgs::msg::AGVEvent::ALARM;
    slam_event_msg.ackreq = true;
  } else if (!is_laser_data_ok_) {
    slam_event_msg.description = "laser data is timedout!";
    slam_event_msg.eventtime = stamptmp.seconds();
    slam_event_msg.ecode = agv_msgs::msg::AGVEvent::SLAM_LIDAR_DATA_ERROR;
    slam_event_msg.action = agv_msgs::msg::AGVEvent::DELAYSTOP;
    slam_event_msg.type = agv_msgs::msg::AGVEvent::ALARM;
    slam_event_msg.ackreq = true;
  } else if (!is_slam_laser_data_ok_) {
    slam_event_msg.description = "valid laser data is too less!";
    slam_event_msg.eventtime = stamptmp.seconds();
    slam_event_msg.ecode = agv_msgs::msg::AGVEvent::SLAM_LIDAR_DATA_WARNING;
    slam_event_msg.action = agv_msgs::msg::AGVEvent::SLOWDONE;
    slam_event_msg.type = agv_msgs::msg::AGVEvent::WARNING;
    slam_event_msg.ackreq = true;
  } else if (!is_odom_ok_) {
    slam_event_msg.description = "odom data is timedout!";
    slam_event_msg.eventtime = stamptmp.seconds();
    slam_event_msg.ecode = agv_msgs::msg::AGVEvent::SLAM_ODOM_DATA_ERROR;
    slam_event_msg.action = agv_msgs::msg::AGVEvent::DELAYSTOP;
    slam_event_msg.type = agv_msgs::msg::AGVEvent::ALARM;
    slam_event_msg.ackreq = true;
  } else {
    slam_event_msg.description = "";
    slam_event_msg.eventtime = stamptmp.seconds();
    slam_event_msg.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
    slam_event_msg.type = agv_msgs::msg::AGVEvent::INFO;
    slam_event_msg.action = agv_msgs::msg::AGVEvent::NONE;
    slam_event_msg.ackreq = true;
  }
  slam_event_pub_->publish(slam_event_msg);
}

void Node::setSavemapProgress(slam_ros_msgs::msg::MapdataProgress& msg) {
  save_map_progress_msg_ = msg;
}

slam_ros_msgs::msg::MapdataProgress Node::getMapProgress() {
  return save_map_progress_msg_;
}

}  // namespace slam_ros